/////////////////////////////////////////////////////////////////////////
//
// Amuse Engine SDK - core/math
// Copyright (c) 2014.  All Rights Reserved
//
// File:		AEQuaternion.h
// Author:		Gianluca Belardelli
// Date:		31/07/2013
// Purpose:		Implementazione dei metodi inline della classe AEQuaternion
//
// Revision:
//				001 - 31/07/2013
//					+ Revisione iniziale
//
/////////////////////////////////////////////////////////////////////////
#ifndef _AEQUATERNION_INL_
#define _AEQUATERNION_INL_

//#include <core/math/utils/AEEulerUtil.h>

AE_FORCEINLINE const AEQuaternion AEQuaternion::IdentityQuaternion( void ) 
{ 
	AEQuaternion qtIdentity; 
	qtIdentity.SetIdentity(); 
	return qtIdentity; 
}

AE_FORCEINLINE void AEQuaternion::SetIdentity( void )
{
	fX = 0.0f;
	fY = 0.0f;
	fZ = 0.0f;
	fW = 1.0f;
}

AE_FORCEINLINE void AEQuaternion::Normalize( void )
{
	const float fLengthSQR = GetLengthSquared();
	AEASSERT_MSG( !AEMath::IsZero( fLengthSQR ), "The length of the quaternion must not be zero." );
	
	const float fLengthInv = AEMath::SqrtInverse( fLengthSQR );
	
	fX *= fLengthInv;
	fY *= fLengthInv;
	fZ *= fLengthInv;
	fW *= fLengthInv;
}

AE_FORCEINLINE bool AEQuaternion::IsNormalized( float fEpsilon ) const
{
	return AEMath::IsFloatEqual( GetLengthSquared(), 1.0f, fEpsilon );
}

AE_FORCEINLINE void AEQuaternion::SetFromMat3( const AEMatrix3 &matRotation )
{
	const float fTrace = matRotation.m_fColumn[0][0] + matRotation.m_fColumn[1][1] + matRotation.m_fColumn[2][2];
	const float fHalf = 0.5f;
	
	float fValues[4];
	
	if( fTrace > 0.0f )
	{
		float s = AEMath::Sqrt( fTrace + 1.0f );
		float t = fHalf / s;
		
		fValues[0] = ( matRotation.m_fColumn[1][2] - matRotation.m_fColumn[2][1] ) * t;
		fValues[1] = ( matRotation.m_fColumn[2][0] - matRotation.m_fColumn[0][2] ) * t;
		fValues[2] = ( matRotation.m_fColumn[0][1] - matRotation.m_fColumn[1][0] ) * t;
		
		fValues[3] = fHalf * s;
	}
	else
	{
		const AEINT32 next[] = {1,2,0};
		AEINT32 i = 0;
		
		if( matRotation.m_fColumn[1][1] > matRotation.m_fColumn[0][0] ) 
			i = 1;
		
		if( matRotation.m_fColumn[2][2] > matRotation.m_fColumn[i][i] ) 
			i = 2;
		
		AEINT32 j = next[i];
		AEINT32 k = next[j];
		
		float s = AEMath::Sqrt( matRotation.m_fColumn[i][i] - (matRotation.m_fColumn[j][j] + matRotation.m_fColumn [k][k] ) + 1.0f );
		float t = fHalf / s;
		
		fValues[i] = fHalf * s;
		fValues[3] = ( matRotation.m_fColumn[j][k] - matRotation.m_fColumn[k][j] ) * t;
		fValues[j] = ( matRotation.m_fColumn[i][j] + matRotation.m_fColumn[j][i] ) * t;
		fValues[k] = ( matRotation.m_fColumn[i][k] + matRotation.m_fColumn[k][i] ) * t;
	}
	
	fX = fValues[0];
	fY = fValues[1];
	fZ = fValues[2];
	fW = fValues[3];
}

AE_FORCEINLINE void AEQuaternion::Invert( void )
{
	fW = -fW;
}

AE_FORCEINLINE const AEQuaternion AEQuaternion::operator- ( void ) const
{
	AEQuaternion qtTemp;
	qtTemp.fX = fX;
	qtTemp.fY = fY;
	qtTemp.fZ = fZ;
	qtTemp.fW = -fW;
	
	return qtTemp;
}

AE_FORCEINLINE float AEQuaternion::GetLength( void ) const
{
	return AEMath::Sqrt( GetLengthSquared() );
}

AE_FORCEINLINE float AEQuaternion::GetLengthSquared( void ) const
{
	return this->Dot( *this );
}

AE_FORCEINLINE float AEQuaternion::Dot( const AEQuaternion &qtRhs ) const
{
	return ( fX * qtRhs.fX + fY * qtRhs.fY + fZ * qtRhs.fZ + fW * qtRhs.fW );
}

AE_FORCEINLINE bool AEQuaternion::IsIdentical( const AEQuaternion &qtRhs ) const
{
	const AEMatrix3 matLeft = GetAsMat3();
	const AEMatrix3 matRight = qtRhs.GetAsMat3();
	
	return matLeft.IsIdentical( matRight );
}

AE_FORCEINLINE bool AEQuaternion::IsEqual( const AEQuaternion &qtRhs, float fEpsilon ) const
{
	AEASSERT( fEpsilon >= 0.0f );
	
	const AEMatrix3 matLeft = GetAsMat3();
	const AEMatrix3 matRight = qtRhs.GetAsMat3();
	
	return matLeft.IsEqual( matRight, fEpsilon );
}

AE_FORCEINLINE void AEQuaternion::SetFromAxisAndAngle( const AEVector3 &vcAxis, float fAngleDeg )
{
	AEASSERT_MSG( vcAxis.IsNormalized(), "The rotation axis must be normalized." );
	
	const float fHalfAngleRad = 0.5f * AEMath::Degrees2Radians( fAngleDeg );
	
	float fSinHalf = AEMath::Sin( fHalfAngleRad );
	
	fX = fSinHalf * vcAxis.fX;
	fY = fSinHalf * vcAxis.fY;
	fZ = fSinHalf * vcAxis.fZ;
	fW = AEMath::Cos( fHalfAngleRad );
}

AE_FORCEINLINE void AEQuaternion::SetFromEulerAngles( float fRollDeg, float fPitchDeg, float fYawDeg )
{
	*this = AEEulerUtil::ConvertEulerToQuaternion_Deg( fRollDeg, fPitchDeg, fYawDeg, AEEulerMode::AEEngineDefault );
}

AE_FORCEINLINE void AEQuaternion::GetAsEulerAngles( float &fRollDeg, float &fPitchDeg, float &fYawDeg )
{
	AEEulerUtil::ConvertQuaternionToEuler_Deg( *this, fRollDeg, fPitchDeg, fYawDeg, AEEulerMode::AEEngineDefault );
}

AE_FORCEINLINE void AEQuaternion::GetAxisAndAngle( AEVector3 &vcRotationAxis, float &fAngle ) const
{
	if( fX * fX + fY * fY + fZ * fZ <= AEMATH_EPSILON )
	{
		vcRotationAxis.Set( 1, 0, 0 );
		fAngle = 0.0f;
		return;
	}
	
	AEVector3 vcAxisTemp( fX, fY, fZ );
	vcAxisTemp.Normalize();
	
	if( fW < 0 )
		vcRotationAxis = -vcAxisTemp;
	else
		vcRotationAxis = vcAxisTemp;
	
	// angle
	const float fAbsangle = AEMath::Abs( fW );
	const float fAngleTemp = AEMath::Acos( fAbsangle );
	fAngle = AEMath::Radians2Degrees( fAngleTemp * 2.0f );
}

AE_FORCEINLINE const AEQuaternion AEQuaternion::Multiply( const AEQuaternion &qtRhs ) const
{
	const AEVector3 vcLhs( fX, fY, fZ );
	const AEVector3 vcRhs( qtRhs.fX, qtRhs.fY, qtRhs.fZ );
	const float fLhsReal = fW;
	const float fRhsReal = qtRhs.fW;
	
	AEVector3 vcRes;
	vcRes = vcLhs.Cross( vcRhs );
	vcRes += fLhsReal * vcRhs;
	vcRes += fRhsReal * vcLhs;
	
	const float fResW = ( fLhsReal * fRhsReal ) - vcLhs.Dot( vcRhs );
	
	AEQuaternion qtResult;
	qtResult.fX = vcRes.fX;
	qtResult.fY = vcRes.fY;
	qtResult.fZ = vcRes.fZ;
	qtResult.fW = fResW;
	
	return qtResult;
}

AE_FORCEINLINE const AEQuaternion AEQuaternion::MultiplyReverse( const AEQuaternion &qtRhs ) const
{
	return qtRhs.Multiply( *this );
}

AE_FORCEINLINE const AEVector3 AEQuaternion::Transform( const AEVector3 &qtRhs ) const
{
	const AEVector3 vcQuatImg( fX, fY, fZ );
	
	float fReal = fW;
	float fReal2 = fReal*fReal - 0.5f;
	
	AEVector3 vcRet = qtRhs * fReal2;
	
	const float fDotRes = vcQuatImg.Dot( qtRhs );
	
	vcRet += vcQuatImg * fDotRes; 
	
	AEVector3 vcCrossed = vcQuatImg.Cross( qtRhs );
	vcRet += vcCrossed * fReal;
	
	return ( vcRet + vcRet );
}

AE_FORCEINLINE const AEVector3 AEQuaternion::TransformReverse( const AEVector3 &qtRhs ) const
{
	const AEVector3 vcQuatImg( fX, fY, fZ );
	
	float fReal = -fW;
	float fReal2 = fReal*fReal - 0.5f;
	
	AEVector3 vcRet = qtRhs * fReal2;
	
	const float fDotRes = vcQuatImg.Dot( qtRhs );
	
	vcRet += vcQuatImg * fDotRes; 
	
	AEVector3 vcCrossed = vcQuatImg.Cross( qtRhs );
	vcRet += vcCrossed * fReal;
	
	return ( vcRet + vcRet );
}

AE_FORCEINLINE const AEVector3 operator* ( const AEQuaternion &qtLhs, const AEVector3 &vcRhs )
{
	return qtLhs.Transform( vcRhs );
}

AE_FORCEINLINE const AEQuaternion operator* ( const AEQuaternion &qtLhs, const AEQuaternion &qtRhs )
{
	return qtLhs.Multiply( qtRhs );
}

AE_FORCEINLINE bool operator== ( const AEQuaternion &qtLhs, const AEQuaternion &qtRhs )
{
	return qtLhs.IsIdentical( qtRhs );
}

AE_FORCEINLINE bool operator!= ( const AEQuaternion &qtLhs, const AEQuaternion &qtRhs )
{
	return !qtLhs.IsIdentical( qtRhs );
}

AE_FORCEINLINE const AEMatrix3 AEQuaternion::GetAsMat3( void ) const
{
	const float fXX =  fX * fX * 2.0f;
	const float fYY =  fY * fY * 2.0f;
	const float fZZ =  fZ * fZ * 2.0f;
	const float fXY =  fX * fY * 2.0f;
	const float fXZ =  fX * fZ * 2.0f;
	const float fYZ =  fY * fZ * 2.0f;
	const float fWX =  fW * fX * 2.0f;
	const float fWY =  fW * fY * 2.0f;
	const float fWZ =  fW * fZ * 2.0f;
	
	AEMatrix3 matRes;
	
	// fX axis
	matRes.m_fColumn[0][0] = 1.0f - ( fYY + fZZ );
	matRes.m_fColumn[1][0] = fXY - fWZ;
	matRes.m_fColumn[2][0] = fXZ + fWY;
	
	// fY axis
	matRes.m_fColumn[0][1] = fXY + fWZ;
	matRes.m_fColumn[1][1] = 1.0f - ( fXX + fZZ );
	matRes.m_fColumn[2][1] = fYZ - fWX;
	
	// fZ axis
	matRes.m_fColumn[0][2] = fXZ - fWY;
	matRes.m_fColumn[1][2] = fYZ + fWX;
	matRes.m_fColumn[2][2] = 1.0f - ( fXX + fYY );
	
	return matRes;
}

AE_FORCEINLINE const AEMatrix4 AEQuaternion::GetAsMat4( void ) const
{
	return AEMatrix4( GetAsMat3(), AEVector3::ZeroVector() );
}

AE_FORCEINLINE bool AEQuaternion::IsIdentity( float fEpsilon ) const
{
	return AEMath::IsFloatEqual( fX, 0, fEpsilon ) && 
			AEMath::IsFloatEqual( fY, 0, fEpsilon ) &&
			AEMath::IsFloatEqual( fZ, 0, fEpsilon ) &&
			( AEMath::IsFloatEqual( fW, 1, fEpsilon ) || AEMath::IsFloatEqual( fW, -1, fEpsilon ) );
}

AE_FORCEINLINE void AEQuaternion::FlipSigns( void )
{
	fX = -fX;
	fY = -fY;
	fZ = -fZ;
	fW = -fW;
}

AE_FORCEINLINE bool AEQuaternion::IsValid( void ) const
{
	return ( AEMath::IsFiniteNumber( fX ) && 
			 AEMath::IsFiniteNumber( fY ) &&
			 AEMath::IsFiniteNumber( fZ ) &&
			 AEMath::IsFiniteNumber( fW ) );
}

AE_FORCEINLINE void AEMatrix4::SetFromQuaternion( const AEQuaternion &qtValue )
{
	*this = qtValue.GetAsMat4();
}

AE_FORCEINLINE AEQuaternion AEMatrix4::GetAsQuaternion( void ) const
{
	return GetRotationalPart().GetAsQuaternion();
}

#endif
// _AEQUATERNION_INL_
